//
// Created by qinf on 2025/7/27.
//
#include "generateData.h"
#include <iostream>
#include <vector>
#include <cstring>
#include <iomanip>

using namespace std;

void parseTaskData(const vector<uint8_t>& data) {
    if (data.size() < 103) {
        cout << "Invalid Task Data size" << endl;
        return;
    }

    for (int i = 0; i < 4; i++) {
        int offset = 2 + i * 25;
        uint32_t droneID = 0, targetID = 0;
        double longitude = 0, latitude = 0;

        memcpy(&droneID, &data[offset], 4);
        memcpy(&targetID, &data[offset + 4], 4);
        memcpy(&longitude, &data[offset + 8], 8);
        memcpy(&latitude, &data[offset + 16], 8);

        printTaskData(droneID, targetID, longitude, latitude);
    }
}

void parseWaypointData(const vector<uint8_t>& data) {
    if (data.size() < 7) {
        cout << "Invalid Waypoint Data size" << endl;
        return;
    }

    uint8_t droneID = data[2];
    int32_t numPoints = 0;
    memcpy(&numPoints, &data[3], 4);

    vector<Waypoint> waypoints;

    for (int i = 0; i < numPoints; i++) {
        if (7 + i * 28 + 28 > data.size()) {
            cout << "Waypoint data truncated" << endl;
            break;
        }

        Waypoint wp;
        memcpy(&wp.id, &data[7 + i * 28], 4);
        memcpy(&wp.longitude, &data[7 + i * 28 + 4], 8);
        memcpy(&wp.latitude, &data[7 + i * 28 + 12], 8);
        memcpy(&wp.altitude, &data[7 + i * 28 + 20], 8);

        waypoints.push_back(wp);
    }

    printWaypointData(droneID, numPoints, waypoints);
}

void parseTargetData(const vector<uint8_t>& data) {
    if (data.size() < 26) {
        cout << "Invalid Target Data size" << endl;
        return;
    }

    double radiation = 0, longitude = 0, latitude = 0;
    memcpy(&radiation, &data[2], 8);
    memcpy(&longitude, &data[10], 8);
    memcpy(&latitude, &data[18], 8);

    printTargetData(radiation, longitude, latitude);
}

void parseData(const vector<uint8_t>& data) {
    if (data.size() < 2 || data[0] != FRAME_HEADER1 || data[1] != FRAME_HEADER2) {
        cout << "Invalid frame header!" << endl;
        return;
    }

    switch (data.size()) {
        case 103:
            parseTaskData(data);
            break;
        default:
            if (data.size() > 26 && data[2] <= 4) {
                parseWaypointData(data);
            } else {
                parseTargetData(data);
            }
            break;
    }
}
